#include <stdio.h>
#include <string.h>
#include <avr/io.h>
#define F_CPU 16000000UL  // 16 MHz
#include <util/delay.h>
#include "parani.h"
#include "uart.h"

#define LED_DDR  DDRD
#define LED_PORT PORTD
#define LED_PIN  PIND
#define LED_BIT  PD6


#define SET_BIT(REG, BIT) do {REG |= _BV(BIT);}while(0)
#define CLR_BIT(REG, BIT) do {REG &= ~_BV(BIT);}while(0)

static void init_led()
{
	SET_BIT(LED_DDR, LED_BIT);
}
static void led_on()
{
	SET_BIT(LED_PORT, LED_BIT);
}
static void led_off()
{
	CLR_BIT(LED_PORT, LED_BIT);
}

static void error_loop(char * msg, int err, int msec)
{
	for(;;) {
		printf("err[%d][%s]", err, msg);
		printf("a");
		printf(CR);
		_delay_ms(msec);
		led_off();
		_delay_ms(msec);
		led_on();
	}
}

//char s[600];
#define RESP_LEN 100
char resp[RESP_LEN];
int main(void)
{
	int err = 0;
	char id[13];
	uart_init();
	init_led();
	led_on();

	_delay_ms(2000);
	led_off();

//	err = parani_reset_factory_defaults();
	if (err != 0)
		error_loop("factory", err, 3000);

	err = parani_init();
	if (err != 0)
		error_loop("init-fail", err, 6000);

	//err = parani_get_all_registers(s, 300);
	if (err != 0)
		error_loop("get-all-registers", err, 500);
	
	//err = parani_set_class_of_device_from_manual();
	err = parani_set_class_of_devicetoy_robot();
	//err = parani_set_class_of_device001101();
	if (err != 0)
		error_loop("set-cod-man", err, 500);

	//err = parani_get_all_registers(s, 300);
	if (err != 0)
		error_loop("get-all-registers2", err, 500);

	for(;;) {
		led_off();
		_delay_ms(100);
		led_on();
		err = parani_wait_for_connections_forever(id);
		if (err != 0)
			error_loop("wait-for-conn", err, 2000);
		int on = 1;
		for(;;) {
			err = parani_get_response(resp, RESP_LEN);
			if (err != 0)
				break;
			
			if (strcmp(resp, "DISCONNECT") == 0) {
				// the other end point closed the connection
				break;
			}
			// if the answer was not a disconnect, 
			// try to convert it to a (int)
			//sscanf(resp, "%d", &on);
			on = strcmp(resp, "STOP");
			if (on)
				led_on();
			else
				led_off();
			//printf("crap reg= %s\n", s);
			//printf("+++\r");
		}
		
		if (err != 0)
			break;
	}
	error_loop("after infinite", 555, 5000);
}
